#pragma once

#include "common.h"
#include "rigidbody.h"

namespace TorPhysix
{
	// A contact represents two bodies in contact. Resolving a contact means removing interpenetration and applying the impulse needed
	// to keep them apart.  In case an active body has collided with a static object (ground or level geometry) 
	// the contact will only contain one body, the other will be NULL.
	// A contact can't do anything useful on its own, it needs to be operated on by the Contact Resolver.
	class Contact
	{
		// Allow the Contact Resolver access to modify the contact
		friend class ContactResolver;
	public:
		// body[1] will be NULL when we only have one non-static body colliding.
		RigidBody *body[2];
		
		// A contact is always oriented in the direction of the collision normal (x-axis), the other two components (y and z) represent friction forces
		// coefficient for lateral friction
		real friction;
		
		// restitution in direction of contact normal (bounciness)
		real restitution; 
		
		// Contact point of bodies in world coordinates
		Vec3f contactPoint;
		
		Vec3f contactNormal;
		
		// Since the contact normal is unit length (its a normal!), we need to store penetration depth
		real penetration;
		
		void setBodyData(RigidBody* one, RigidBody *two, real friction, real restitution);
    protected:


		// Transform matrix from contact coordinates to world coordinates. 
        Mat3f contactToWorld; 

		// closing velocity at the point of contact. is set by calculateInternals
        Vec3f contactVelocity; 
        
		// needed velocity change to resolve contact, it is stored as a real, as we know the direction from the contactToWorld frame of reference
		real desiredDeltaVelocity; 
        
		// contact point relative to each body. is set by calculateInternals
		Vec3f relativeContactPosition[2]; 

		// calculate internals from state data, do before any resolution, called by Resolver before adjusting position and velocity.
		// body data must be set before
		void calculateInternals(real duration); 

		// reverses the contact. swaps bodies and reverses normal. internals must be calculated beforehand. saves us making 2 contacts for each collision
        void swapBodies(); 

		// calculates and sets internal value
        void calculateDesiredDeltaVelocity(real duration);

		// calculates and returns velocity of contact point on body
        Vec3f calculateLocalVelocity(unsigned bodyIndex, real duration); 

		// create orthonormal basis for contact point based on primary friction direction
		// it depends only on the collision normal
        void calculateContactBasis(); 

		// TODO implement applyImpulse
		// apply impulse, return change in velocity
        //void applyImpulse(const Vec3f &impulse, RigidBody *body, Vec3f *velocityChange, Vec3f *rotationChange); 
  
		// apply inertia-weighted impulse based resolution of this contact. Heavier bodies are affected less.
		// it is used by the contact resolver on the most severe velocity (highest speed)
		// @see ContactResolver::adjustVelocity 
		void applyVelocityChange(Vec3f velocityChange[2], Vec3f rotationChange[2]); 

		// apply inertia weighted penetration resolution of this contact.
        void applyPositionChange(Vec3f linearChange[2], Vec3f angularChange[2], real penetration); 

		// calculates impulse needed to resolve contact, uses the inertia tensor of each object involved. 
        Vec3f calculateFrictionlessImpulse(Mat3f *inverseInertiaTensor); 

		// calculates the impulse needed to resolve contact, given a non-zero friction coefficient.
		// @param inverseInertiaTensor the body inertia tensor is used to decide how much of the impulse to apply.
        Vec3f calculateFrictionImpulse(Mat3f *inverseInertiaTensor); 
	};

}